/* 
 * File:   BPDC.cpp
 * Author: thanasis
 * 
 * Created on 01 July 2013, 15:17
 */

#include "bpdc.h"

BPDC::BPDC(int numSensors, int numMotors) 
{
    //Initialization
    reservoirNum=100;
    inputNum=numMotors;
    outputNum=numSensors;
    inputWeights.set(reservoirNum+outputNum,inputNum);
    inputNodes.set(inputNum,0);
    nodes.set(outputNum+reservoirNum,1);
    Matrix error(outputNum,1);
    errorBuff.assign(2,error);
    desiredSens.assign(2, error);
    
    //Initialize weights within the interval [-1,1]
    weights.set(outputNum+reservoirNum,outputNum+reservoirNum);
    
    for (int i=0; i<outputNum+reservoirNum; i++)
    {
        for (int j=0; j<outputNum+reservoirNum; j++)
        { 
        
        double rndNum=(rand()%100)-50 ;
        double w=(rndNum/1000);
        weights.val(i,j)=w;
        }
        
        
    }
    for (int i=outputNum; i<outputNum+reservoirNum; i++)
    {
       
        for (int j=0; j<inputNum; j++)
        { 
           
        double rndNum=(rand()%100)-50 ;
        double w=(rndNum/1000);
        inputWeights.val(i,j)=w;
            
        }
        
        
    }
    //Initialize continous time frame
     dt=1;
     //learning rate
     heta=0.01;
     //regularization
     epsillon=0.005;
}

BPDC::~BPDC() {
}

 double BPDC::squash(double node)
{
 return tanh(node);   
    
}
 
void BPDC::setDesiredOut(Matrix fut_sensor)
{
 
    
    
    for (int i=0; i<outputNum; i++)
     {  
         desiredSens[1].val(i,0)=fut_sensor.val(i,0);
     }
   
 
}

void BPDC::setState(Matrix motors)
 {      

        inputNodes=motors;
        

 }

Matrix BPDC::simulateNet()
{
    Matrix updatedNodes;
    
   
    updatedNodes=nodes*(1-dt)+((weights*nodes.map(&squash)*dt)+
                       (inputWeights*inputNodes*dt));
      
    return updatedNodes;
           

}

int BPDC::kronneckerDelta (int i, int k)
 {
     if (i==k) return 1;
     else return 0;
 }
double BPDC::squash_derivative(double node)
{
 double k=tanh(node);
    return 1.2 - k*k;
    
}

Matrix BPDC::getModelMatrix ( Matrix unused)
{
    
     //DPartition of Weights matrix 
    Matrix inputW=inputWeights.rows(outputNum,outputNum+reservoirNum-1);
    Matrix inputs=inputNodes;
    Matrix ESNWeights =weights.rows(outputNum,outputNum+reservoirNum-1);
    ESNWeights=ESNWeights.columns(outputNum,outputNum+reservoirNum-1);
   
    Matrix ESNState= nodes.rows(outputNum,outputNum+reservoirNum-1);
    ESNState.map(&squash);
    
    Matrix outputWeights=weights.columns(outputNum,outputNum+reservoirNum-1);
    outputWeights=outputWeights.rows(0,outputNum-1);
    
    Matrix outNodes=nodes.rows(0,outputNum-1);
    outNodes=outNodes.map(&squash);
    
    Matrix backWeights=weights.rows(outputNum,outputNum+reservoirNum-1);
    backWeights=backWeights.columns(0,outputNum-1);
    
   //Calculate jacobian
    
    Matrix internalState=inputW*inputs+ESNWeights*ESNState+backWeights*outNodes;
    
    Matrix g_prime =internalState.map(&squash_derivative);
 
    return ((outputWeights * (inputW & g_prime)));
  
}
 
void BPDC::updateWeights()
{    //BPDC learning rule
        double sum,denominator,gamma;
        Matrix weightsChange(weights.getM(),weights.getN());
    for (int i=0; i<outputNum; i++)
    {
        sum=0;
        denominator=0;
        gamma=0;
        for (int s=0; s<outputNum; s++)
        {
            sum=sum+((((1-dt)*kronneckerDelta(i,s))+
               (weights.val(i,s)*squash_derivative(nodes.val(s,0))*dt))*
                 errorBuff[0].val(s,0));  
            denominator=denominator+((squash(nodes.val(s,0)))*(squash(nodes.val(s,0))));
         
        }
       
       
       gamma=sum-errorBuff[1].val(i,0);
        for (int j=0; j<outputNum+reservoirNum; j++)
        {
        
        weightsChange.val(i,j)=((heta/dt)*((squash(nodes.val(j,0)))/
                ((denominator+epsillon))*gamma));
       
        }
       
    }
        
        weights=weights+weightsChange;
      
}
Matrix BPDC::predict()
{
   //History of errors at the last two time steps
    errorBuff[0]=nodes.rows(0,outputNum-1)-desiredSens[0].rows(0,outputNum-1);
    
    Matrix updatedNodes=simulateNet();
    //Simply swap
    errorBuff[1]=updatedNodes.rows(0,outputNum-1)-desiredSens[1].rows(0,outputNum-1);
    
    updateWeights();
    desiredSens[0]=desiredSens[1];
    nodes=updatedNodes;
    //Return out nodes
    return nodes.rows(0,outputNum-1);
}
